#include "iirfilter.h"
#include <stdio.h>
#include <cmath>
#include <iostream>
#include <cstring>

/**
 * Init the filter operation
 */
iirfilter::iirfilter()
    : ringBuffer_out(0),
      ringBuffer_in(0),
      ringBufferSize_i(0),
      ringBufferSize_o(0),
      idx_out(0),
      idx_in(0),
      out_tmp(0)
{

}

/**
 * Destructor
 */
iirfilter::~iirfilter(){
    delete[] ringBuffer_out;
    delete [] ringBuffer_in;
    delete [] out_tmp;
    ringBufferSize_o = 0;
    ringBufferSize_i = 0;
}


/**
 * Init the filter operation
 */
void iirfilter::init(tIirMainStructure* coefss)
{
    /**
    Init Value for the Coeffs*/
    coefss->coefs_xn[0] = iir_init_coefs[0];
    coefss->coefs_xn[1] = iir_init_coefs[1];
    coefss->coefs_xn[2] = iir_init_coefs[2];
    coefss->coefs_xn[3] = iir_init_coefs[3];

    coefss->coefs_yn[0] = iir_init_coefs[4];
    coefss->coefs_yn[1] = iir_init_coefs[5];
    coefss->coefs_yn[2] = iir_init_coefs[6];
    
    /**
     * find the next base-2 number that can hold the required sample number
     * for Output buffer
     * and Input buffer.
     */

    ringBufferSize_o =  1 << static_cast<int>(ceil(log(IIR_ORDER+1)/log(2.0f)));
    ringBufferSize_i = 1 << static_cast<int>(ceil(log(IIR_ORDER+2)/log(2.0f)));
    delete[] ringBuffer_in;
    delete [] ringBuffer_out;
    delete [] out_tmp;
    out_tmp = new float[1];
    ringBuffer_in = new float[ringBufferSize_i];
    ringBuffer_out = new float[ringBufferSize_o];

    /* initial conditions 0 */
    memset(ringBuffer_in,0,sizeof(float)*ringBufferSize_i);
    memset(ringBuffer_out,0,sizeof(float)*ringBufferSize_o);
    memset(out_tmp,0,sizeof(float));
    idx_in  = 0;
    idx_out = 0;
}

/**
 * IIRFILTER
 */
void iirfilter::filter(
    int blockSize,
    float* in,
    float* out,
    tIirMainStructure  coefss)
{   

    int nmk_o;
    int nmk_i;
    int delay_o;
    int delay_i;
    int i;
    float alpha[3];
    float beta[4];

    const int mask_o = ringBufferSize_o-1;
    const int mask_i = ringBufferSize_i-1;

     /* This will hold value for every y[n] */
    *out_tmp = 0;

    beta[0]  = coefss.coefs_xn[3];
    alpha[0] = coefss.coefs_yn[2];

    beta[1]  = coefss.coefs_xn[2];
    alpha[1] = coefss.coefs_yn[1];

    beta[2]  = coefss.coefs_xn[1];
    alpha[2] = coefss.coefs_yn[0];

    beta[3]  = coefss.coefs_xn[0];

    for (int n=0;n<blockSize;n++)
    {
        /* the following works because the ringBuffer was set with a size
         * equal to 2^n.
         */
        nmk_o = (idx_out-(3)) & mask_o;
        nmk_i = (idx_in-(4)) & mask_i;
        ringBuffer_in[idx_in]=in[n];

        /* Iterates for output samples */
        for (i = 0;i <= 2;i++)
        {
            delay_o= nmk_o + i;

            if (delay_o > mask_o)
            {
                delay_o = delay_o-mask_o - 1;
            }

            out_tmp[0] += alpha[i] * ringBuffer_out[delay_o];
        }

        /* Iterates for input samples*/
        for (i = 0;i <= 3;i++)
        {
            delay_i = nmk_i + i;

            if (delay_i>mask_i)
            {
                delay_i= delay_i-mask_i - 1;
            }

            out_tmp[0] += beta[i] * ringBuffer_in[delay_i];
        }

        out[n]     = out_tmp[0];
        out_tmp[0] = 0;
        ringBuffer_out[idx_out]= out[n];

        idx_out= (idx_out+1)&mask_o;
        idx_in = (idx_in+1)&mask_i;
    }
}

/*
 * Reset
 */
void iirfilter::reset(tIirMainStructure coefss)
{
    coefss.coefs_xn[0] = iir_init_coefs[0];
    coefss.coefs_xn[1] = iir_init_coefs[1];
    coefss.coefs_xn[2] = iir_init_coefs[2];
    coefss.coefs_xn[3] = iir_init_coefs[3];

    coefss.coefs_yn[0] = iir_init_coefs[4];
    coefss.coefs_yn[1] = iir_init_coefs[5];
    coefss.coefs_yn[2] = iir_init_coefs[6];
}

